/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-15 03:26:31
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-07 13:37:09
 * @Description:
 * @Reference:
 */
#include "lci_slam/vilens/pipeline_online.h"

namespace vilens {

onlinePipeline::onlinePipeline(const std::string &config_path) {
    loadParameter(config_path);
    allocateMemory();
    registerPublisher();
}

void onlinePipeline::loadParameter(const std::string &config_path) {
    pipeline_config.reset(new config());
    pipeline_config->load(config_path);
    pipeline_config->print();
}

void onlinePipeline::allocateMemory() {
    // Lidar
    lidar_preprocess_ptr.reset(new lidarPreprocessor());
    lidar_preprocess_ptr->set_config(pipeline_config->lidar.feature_extract,
                                     pipeline_config->lidar.lidar_type,
                                     pipeline_config->lidar.blind,
                                     pipeline_config->lidar.pfilt_num);

    lidar_process_ptr.reset(new lidarProcessor(pipeline_config));

    // Camera
    camera_process_ptr.reset(new cameraProcessor());
    camera_process_ptr->setConfig();
}

void onlinePipeline::registerPublisher() {
    // Imu
    subImu = nh.subscribe<sensor_msgs::Imu>(pipeline_config->imu.imuTopic, 2000,
                                            &onlinePipeline::imuCallback, this,
                                            ros::TransportHints().tcpNoDelay());

    // Camera
    if (pipeline_config->camera.camNum == 2) {
        auto image_sub0 =
            std::unique_ptr<message_filters::Subscriber<sensor_msgs::Image>>(
                new message_filters::Subscriber<sensor_msgs::Image>(
                    nh, pipeline_config->camera.imageTopics[0], 5));
        auto image_sub1 =
            std::unique_ptr<message_filters::Subscriber<sensor_msgs::Image>>(
                new message_filters::Subscriber<sensor_msgs::Image>(
                    nh, pipeline_config->camera.imageTopics[1], 5));
        auto sync = std::unique_ptr<message_filters::Synchronizer<syncPol>>(
            new message_filters::Synchronizer<syncPol>(syncPol(5), *image_sub0,
                                                       *image_sub1));
        sync->registerCallback(
            boost::bind(&onlinePipeline::stereoCallback, this, _1, _2, 0, 1));
        // Append to our vector of subscribers
        syncCam.push_back(std::move(sync));
        syncSubCam.push_back(std::move(image_sub0));
        syncSubCam.push_back(std::move(image_sub1));
    } else {
        // Now we should add any non-stereo callbacks here
        for (int i = 0; i < pipeline_config->camera.camNum; i++) {
            // create subscriber
            subImages.push_back(nh.subscribe<sensor_msgs::Image>(
                pipeline_config->camera.imageTopics[i], 5,
                boost::bind(&onlinePipeline::monocularCallback, this, _1, i)));
        }
    }

    // Lidar
    subLidar = lidar_preprocess_ptr->lidar_type == 1
                   ? nh.subscribe<lci_slam::CustomMsg>(
                         pipeline_config->lidar.lidarTopic, 200000,
                         &onlinePipeline::livoxPclCallback, this,
                         ros::TransportHints().tcpNoDelay())
                   : nh.subscribe<sensor_msgs::PointCloud2>(
                         pipeline_config->lidar.lidarTopic, 200000,
                         &onlinePipeline::standardPclCallback, this,
                         ros::TransportHints().tcpNoDelay());
    std::cout << "lidar_preprocess_ptr->lidar_type : "
              << pipeline_config->lidar.lidarTopic << std::endl;

    // Restart
    subRestart = nh.subscribe<std_msgs::Bool>(
        "restart", 100, &onlinePipeline::restartCallback, this);

    sigBuffer.notify_all();
}

void onlinePipeline::standardPclCallback(
    const sensor_msgs::PointCloud2::ConstPtr &lidar_msg) {
    if (lidar_msg->header.stamp.toSec() < last_timestamp_lidar) {
        ROS_ERROR("lidar loop back, clear buffer");
        mtx_data_buffer.lock();
        while (!lidar_data_buffer.empty())
            lidar_data_buffer.pop();
        mtx_data_buffer.unlock();
    }

    PointCloudXYZI::Ptr lidar_data(new PointCloudXYZI());
    lidar_preprocess_ptr->process(lidar_msg, lidar_data);

    mtx_data_buffer.lock();
    {
        last_timestamp_lidar = lidar_msg->header.stamp.toSec();
        lidar_data_buffer.push(std::pair<double, PointCloudXYZI::Ptr>(
            last_timestamp_lidar, lidar_data));
    }
    mtx_data_buffer.unlock();
    sigBuffer.notify_all();
}

void onlinePipeline::livoxPclCallback(
    const lci_slam::CustomMsg::ConstPtr &lidar_msg) {
    if (lidar_msg->header.stamp.toSec() < last_timestamp_lidar) {
        ROS_ERROR("lidar loop back, clear buffer");
        mtx_data_buffer.lock();
        while (!lidar_data_buffer.empty())
            lidar_data_buffer.pop();
        mtx_data_buffer.unlock();
    }

    PointCloudXYZI::Ptr lidar_data(new PointCloudXYZI());
    lidar_preprocess_ptr->process(lidar_msg, lidar_data);

    mtx_data_buffer.lock();
    {
        last_timestamp_lidar = lidar_msg->header.stamp.toSec();
        lidar_data_buffer.push(std::pair<double, PointCloudXYZI::Ptr>(
            last_timestamp_lidar, lidar_data));
    }
    mtx_data_buffer.unlock();
    sigBuffer.notify_all();
}

void onlinePipeline::imuCallback(const sensor_msgs::ImuConstPtr &imu_msg) {
    if (imu_msg->header.stamp.toSec() <= last_timestamp_imu) {
        ROS_WARN("imu message in disorder!");
        return;
    }

    mtx_data_buffer.lock();
    {
        last_timestamp_imu = imu_msg->header.stamp.toSec();
        imu_data_buffer.push(std::pair<double, sensor_msgs::ImuConstPtr>(
            last_timestamp_imu, imu_msg));
    }
    mtx_data_buffer.unlock();
    sigBuffer.notify_all();
}

void onlinePipeline::stereoCallback(const sensor_msgs::ImageConstPtr &img_msg0,
                                    const sensor_msgs::ImageConstPtr &img_msg1,
                                    int cam_id0, int cam_id1) {
    mtx_data_buffer.lock();

    camera_process_ptr->processStereo(
        img_msg0->header.stamp.toSec(), getImageFromMsg(img_msg0),
        getImageFromMsg(img_msg1), cam_id0, cam_id1);

    mtx_data_buffer.unlock();
    sigBuffer.notify_all();
}

void onlinePipeline::monocularCallback(
    const sensor_msgs::ImageConstPtr &img_msg, int cam_id) {
    mtx_data_buffer.lock();

    camera_process_ptr->processMono(img_msg->header.stamp.toSec(),
                                    getImageFromMsg(img_msg), cam_id);

    mtx_data_buffer.unlock();
    sigBuffer.notify_all();
}

void onlinePipeline::restartCallback(
    const std_msgs::BoolConstPtr &restart_msg) {
    std::cout << "restart the estimator!" << std::endl;
    if (restart_msg->data == true) {
        reset();
    }
    sigBuffer.notify_all();
}

int onlinePipeline::process() {
    lidar_process();

    return 0;
}

int onlinePipeline::lidar_process() {
    while (1) {
        std::pair<double, PointCloudXYZI::Ptr> lidar_data;

        // std::cout << lidar_data_buffer.size() << std::endl;
        // std::cout << imu_data_buffer.size() << std::endl;

        if (!lidar_data_buffer.empty()) {
            mtx_data_buffer.lock();
            {
                lidar_data = lidar_data_buffer.front();
                lidar_data_buffer.pop();
            }
            mtx_data_buffer.unlock();

            lidar_process_ptr->process(lidar_data.first, lidar_data.second);
        }
    }

    return 0;
}

void onlinePipeline::reset() {}

}  // namespace vilens
